#include "RMRAccel.h"
#include "RMRUtil.h"


// constructor based on AnalogChannel
// (needed for direct acess to the accelerometer voltage
RMRAccel::RMRAccel(AnalogChannel *channel) :
	Accelerometer(channel)
{
	m_analogchannel = channel;
	m_gain = ACCEL_GAIN;
	m_acceleration_filtered = 0.0;
}

// utility routine to set member variables to defaults
void RMRAccel::SetDefaults(void)
{
	m_last_t = GetClock();
	m_last_speed = 0.0;
}


// take CALIBRATION_SAMPLES samples from the accelerometer to find the zero point
// this takes CALIBRATION_WAIT seconds
// (we assume the robot is stationary)
void RMRAccel::Calibrate(Watchdog &watchdog)
{
	float old_expiration = watchdog.GetExpiration();
	watchdog.SetExpiration(MAX_TIME_FOR_CALIBRATE);
	
	int n;
	float sum, sample = 0.0;
	float wait_time = CALIBRATION_WAIT / CALIBRATION_SAMPLES;
	
	for (n = 0; n < CALIBRATION_SAMPLES; n++)
	{
		sample = m_analogchannel->GetAverageVoltage();
		sum += sample;
		Wait(wait_time);
		watchdog.Feed();
	}
//	printf("found zero point = %8.4f volts\n", sum / 30);
	SetZero(sum / CALIBRATION_SAMPLES);
	ResetSpeed();
	
	watchdog.SetExpiration(old_expiration);
}

// Directly get the voltage from the accelerometer
float RMRAccel::GetVoltage(void)
{
	return m_analogchannel->GetAverageVoltage();
}

// reset the speed to zero (in case we know the accelerometer isn't moving)
void RMRAccel::ResetSpeed()
{
	m_last_speed = 0.0;
}

/*
 * To Do:
 * 
 * Make this run on a Notifier class (like PressAndHoldButton), scheduled to run every 0.05 or so seconds
 * 
 * Then gave the GetSpeed just return the calculated speed
 */
// return the current accelerometer speed in units of feet/sec
// found by integrating accelerometer output
float RMRAccel::GetSpeed(void)
{
	static int c = 0;
	int max = 100;
	
	static int i = 0;
	int period = DEBUG_OUT_PERIOD / max;
	
	
	// only run the integration every 100 cycles (so our delta_t values aren't really small)
	c++;
	if (c > max)
	{
		c = 0;
		
		
		// get time now
		float now_t = GetClock();
		
		// find difference in time from last call: delta_t
		float delta_t = now_t - m_last_t;
		
		
		float accel_g = GetFilteredAccel();
		
		if (accel_g < 0.01 && accel_g > - 0.01)
		{
			accel_g = 0;
		}
		
		// debug stuff
		i++;
		if (i > period)
		{
			i = 0;
			//printf("delta_t = %8.4f, acceleration in g's = %8.4f\n", delta_t, accel_g);
		}
		
		// last time is now
		m_last_t = now_t;
		
		// the 32 is g=32.15 ft/s^2
		m_last_speed += delta_t * accel_g * 32.15;
	}
	
	return m_last_speed;

}

// Return the current acceleration (in terms of g), filtered to reduce noise
float RMRAccel::GetFilteredAccel(void)
{
	float acceleration = GetAcceleration();
	m_acceleration_filtered = m_gain * acceleration + (1.0 - m_gain) * m_acceleration_filtered;	
	return m_acceleration_filtered;
}



/* This is the code written in RMRPIDIO, used here only for refrence
 * float PIDEncoder::GetSpeed(void)
{	
	INT32 now_x = Get();
	float now_t = GetClock();
	
	INT32 delta_x = now_x - m_last_x;
	float delta_t = now_t - m_last_t;
	
	//printf("t: last = %f, now = %f\n", m_last_t, now_t);
	//printf("x: last = %d, now = %d\n", m_last_x, now_x);
	
	
	m_last_x = now_x;
	m_last_t = now_t;
	
	
	if (delta_t == 0) return 0;
	
	return delta_x / delta_t * m_scaling;
	
}
*/

